/* 
 * File:   main.c
 * Author: nono
 *
 * Created on 23 janvier 2013, 11:13
 */

#include <stdio.h>
#include <stdlib.h>
#include <math.h>


#define width 100
#define height 100

#define COEFF_POTENTIEL_MUR 1
#define COEFF_POTENTIEL_BUT -99
#define COEFF_FROTTEMENT 0.1

#define XBEGIN 2
#define YBEGIN 2
#define XBUT 95
#define YBUT 95

#define PM 4
#define N 4

double map[width][height];
double potentialMap[width][height];
double gradientMap[width - 1][height - 1][2];

double maxPot = 0;
double minPot = 0;

double q[N];

void init_map() {
    int i = 0;
    int j = 0;
    for (i = 0; i < height; i++) {
        for (j = 0; j < width; j++) {
            map[i][j] = 0;
            potentialMap[i][j] = 0;
        }
    }

    i = 0;
    for (j = 0; j < height; j++) {
        map[i][j] = COEFF_POTENTIEL_MUR;
    }
    i = height - 1;
    for (j = 0; j < height; j++) {
        map[i][j] = COEFF_POTENTIEL_MUR;
    }
    j = 0;
    for (i = 0; i < width; i++) {
        map[i][j] = COEFF_POTENTIEL_MUR;
    }
    j = width - 1;
    for (i = 0; i < width; i++) {
        map[i][j] = COEFF_POTENTIEL_MUR;
    }



    map[50][48] = COEFF_POTENTIEL_MUR;
    map[48][48] = COEFF_POTENTIEL_MUR;
    map[49][48] = COEFF_POTENTIEL_MUR;

    map[XBUT][YBUT] = COEFF_POTENTIEL_BUT;
}

void init_gradients() {
    int i = 0;
    int j = 0;
    for (i = 0; i < height - 1; i++) { // c'est bien -1 sinon ça écrit n'importe où dans la mémoire -> cause de bug chez nono
        for (j = 0; j < width - 1; j++) {
            gradientMap[i][j][0] = -potentialMap[i + 1][j] + potentialMap[i][j];
            gradientMap[i][j][1] = -potentialMap[i][j + 1] + potentialMap[i][j];
        }
    }
}

void init_potentials() {
    int i = 0;
    int j = 0;
    int k = 0;
    int l = 0;
    for (i = 0; i < height; i++) {
        for (j = 0; j < width; j++) {
            for (k = 0; k < height; k++) {
                for (l = 0; l < width; l++) {
                    if (k == i && l == j) {
                        potentialMap[i][j] += map[k][l];
                    } else {
                        // if (map[k][l] == COEFF_POTENTIEL_BUT){
                        //   potentialMap[i][j] += map[k][l] / sqrt((k - i)*(k - i) + (l - j)*(l - j));
                        //}else{
                        potentialMap[i][j] += map[k][l] / sqrt((k - i)*(k - i) + (l - j)*(l - j));
                        //}
                    }
                }
                if (map[i][j] == COEFF_POTENTIEL_MUR) {
                    potentialMap[i][j] = 99;
                } else if (map[i][j] == COEFF_POTENTIEL_BUT) {
                    potentialMap[i][j] = -99;
                }
            }
        }
    }
}

void getMinMaxPot() {
    // on need pour heatmap.js, génération potentiel canvas
    int i = 0;
    int j = 0;
    for (i = 0; i < width; i++) {
        for (j = 0; j < height; j++) {
            if (potentialMap[i][j] > maxPot) {
                maxPot = potentialMap[i][j];
            }
            if (potentialMap[i][j] < minPot) {
                minPot = potentialMap[i][j];
            }
        }
    }
}

void flush_to_file_gradient() {
    FILE * fich = NULL;
    fich = fopen("dats/gradient.dat", "w");
    int i = 0;
    int j = 0;
    for (i = 0; i < width - 1; i++) {
        fprintf(fich, "-- Colonne %d --\n", i);
        for (j = 0; j < height - 1; j++) {
            fprintf(fich, "%3.8lg\t", gradientMap[i][j][0]);
            fprintf(fich, "%3.8lg\t", gradientMap[i][j][1]);
            fprintf(fich, "\n");
        }
    }
    fclose(fich);
}

void flush_to_file_potential() {
    // splot 'potentiel.dat' using 1:2:3 with pm3d => gnuplot cmd
    FILE * fich = NULL;
    fich = fopen("dats/potentiel.dat", "w");
    int i = 0;
    int j = 0;
    for (i = 0; i < width; i++) {
        for (j = 0; j < height; j++) {
            fprintf(fich, "%d %d %lg\n", i, j, potentialMap[i][j]);
        }
        fprintf(fich, "\n");
    }
    fclose(fich);
}

double getGradient(int k, int l, int d) {
    if (k < 0 || l < 0 || k >= width || l >= height) {
        return 0;
    } else {
        return gradientMap[k][l][d];
    }
}

// Interpolation bilinéaire

double getBallGradient(double x, double y, int d) {
    int dx = width / (width - 2);
    int dy = height / (height - 2);
    int k1 = floor(x / dx);
    int k2 = ceil(x / dx);
    int l1 = floor(y / dy);
    int l2 = ceil(y / dy);

    if (k1 != k2 && l1 != l2) {
        double a = getGradient(k1, l1, d);
        double b = getGradient(k2, l1, d);
        double c = getGradient(k1, l2, d);
        double d = getGradient(k2, l2, d);
        if (d == 0) {
            return a * (k2 - x) * (l2 - y) + b * (x - k1) * (l2 - y) + c * (k2 - x) * (y - l1) + d * (x - k1) * (y - l1) * (1 / dx);
        } else {
            return a * (k2 - x) * (l2 - y) + b * (x - k1) * (l2 - y) + c * (k2 - x) * (y - l1) + d * (x - k1) * (y - l1) * (1 / dy);
        }
    } else if (k1 != k2) { // l1 == l2
        double a = getGradient(k1, l1, d);
        double b = getGradient(k2, l2, d);
        return a + (b - a) * (k2 - x) * (1 / dx);
    } else if (l1 != l2) { // k1 == k2
        double a = getGradient(k1, l1, d);
        double b = getGradient(k2, l2, d);
        return a + (b - a) * (l2 - y) * (1 / dy);
    } else {
        return getGradient(k1, l1, d);
    }
}

void systeme_diff(double q[], double t, double qp[], int n) {
    qp[0] = q[2];
    qp[1] = q[3];
    qp[2] = getBallGradient(q[0], q[1], 0) - COEFF_FROTTEMENT * q[2];
    qp[3] = getBallGradient(q[0], q[1], 1) - COEFF_FROTTEMENT * q[3];
}

void rk4(void(*sd)(double*, double, double*, int), double q[], double t, double dt, int n) {
    int i, j, k, p;
    double *y[PM + 1], *z[PM];
    static const double a[PM][PM] = {1. / 2, 0, 0, 0, 0, 1. / 2, 0, 0, 0, 0, 1, 0, 1. / 6, 1. / 3, 1. / 3, 1. / 6};
    static const double b[PM] = {0, 1. / 2, 1. / 2, 1};

    for (i = 0; i < PM + 1; i++) y[i] = malloc(n * sizeof (double));
    for (i = 0; i < PM; i++) z[i] = malloc(n * sizeof (double));

    for (i = 0; i < n; i++) y[0][i] = q[i];
    for (p = 1; p <= PM; p++) {
        sd(y[p - 1], t + b[p - 1] * dt, z[p - 1], n);
        for (i = 0; i < n; i++) y[p][i] = q[i];
        for (k = 0; k < p; k++) {
            for (i = 0; i < n; i++) y[p][i] += dt * a[p - 1][k] * z[k][i];
        }
    }
    for (i = 0; i < n; i++) q[i] = y[PM][i];
}

void flush_to_JSON(double trajectoire[], int size) {
    /** Map **/
    int i = 0;
    int j = 0;
    char enter = 0;
    FILE *fichJSON;
    fichJSON = fopen("web_view/jsons/map-default.json", "w");
    fprintf(fichJSON, "{\"map\":[");
    for (i = 0; i < width; i++) {
        for (j = 0; j < height; j++) {
            if (map[i][j] != 0) {
                if (enter != 0) {
                    fprintf(fichJSON, ",");
                }
                fprintf(fichJSON, "{\"x\": %d, \"y\": %d, \"val\": %lg}", i, j, map[i][j]);
                enter = 1;
            }
        }
    }
    fprintf(fichJSON, "]}");
    fclose(fichJSON);


    /** Potentials **/
    i = 0;
    j = 0;
    enter = 0;
    getMinMaxPot();
    fichJSON = fopen("web_view/jsons/potentials-default.json", "w");
    fprintf(fichJSON, "{\"max\": %lg, \"data\": [", (maxPot - minPot));
    for (i = 0; i < width; i++) {
        for (j = 0; j < height; j++) {
            if (enter != 0) {
                fprintf(fichJSON, ",");
            }
            fprintf(fichJSON, "{\"x\": %d, \"y\": %d, \"count\": %lg}", i, j, (potentialMap[i][j] - minPot));
            enter = 1;
        }
    }
    fprintf(fichJSON, "]}");
    fclose(fichJSON);

    /** Trajectoire **/
    i = 0;
    fichJSON = fopen("web_view/jsons/trajectoire-default.json", "w");
    fprintf(fichJSON, "{\"trajectoire\":[");
    for (i = 0; i < size; i++) {
        if (i != 0) fprintf(fichJSON, ",");
        fprintf(fichJSON, "{\"x\": %lg, \"y\": %lg}", trajectoire[2 * i], trajectoire[2 * i + 1]);
    }
    fprintf(fichJSON, "]}");
    fclose(fichJSON);
}

double getDistance(double xdebut, double xfin, double ydebut, double yfin) {
    return sqrt(((xfin - xdebut)*(xfin - xdebut))+((yfin - ydebut)*(yfin - ydebut)));
}

int main(int argc, char** argv) {
    int i, np, counter, tailleTraj; //Déclarations
    double q[N], q0[N], t, dt, t0, tfin;
    FILE *fich;

    init_map();
    init_potentials();
    flush_to_file_potential();
    init_gradients();
    flush_to_file_gradient();

    /* Calcul trajectoire */
    fich = fopen("dats/trajectoire.dat", "w"); //Fichier
    t0 = 0;
    tfin = 100000;
    np = 1000000;
    q0[0] = XBEGIN; // pos X départ
    q0[1] = YBEGIN; // pos Y départ
    q0[2] = 0; // Vx
    q0[3] = 0; // Vy
    dt = (tfin - t0) / (np - 1); //Pas temps
    
    tailleTraj = (np/100)*2;
    counter = 0;
    double trajectoire[tailleTraj];
    double xavant = 0;
    double yavant = 0;

    for (i = 0; i < N; i++) q[i] = q0[i];
    t = t0; //Amorcage
    for (i = 1; i <= np; i++) {
        int a = i % 100;
        if (a == 0) {
            fprintf(fich, "%lg %lg %lg %lg \n", t, q[0], q[1], q[2]);
            trajectoire[2 * counter] = q[0];
            trajectoire[2 * counter + 1] = q[1];
            counter++;
        }
        xavant = q[0];
        yavant = q[1];
        rk4(systeme_diff, q, t, dt, N);
        if (getDistance(xavant, q[0], yavant, q[1]) > 0.002) {
            dt = dt / 1.2;
            q[0] = xavant;
            q[1] = yavant;
            rk4(systeme_diff, q, t, dt, N);
        } else if (getDistance(xavant, q[0], yavant, q[1]) < 0.0005) {
            dt = dt * 1.2;
            q[0] = xavant;
            q[1] = yavant;
            rk4(systeme_diff, q, t, dt, N);
        }

        //Detection de l'objectif => si on est dedans on bouge plus en conservant le fait qu'on est un tableau de tailleTraj => can't stop the boucle
        if(getDistance(q[0], XBUT, q[1], YBUT) < 0.5){
            q[0] = xavant;
            q[1] = yavant;
            q[2] = 0;
            q[3] = 0;
        }

        //inversion et réduction de la vitesse si on sort des bords 
        if (q[0] >= width || q[1] >= height) {
            q[2] = -q[2]*0.2;
            q[3] = -q[3]*0.2;
        }
        t += dt;
        //printf("%lg %lg\n", dt, getDistance(xavant, q[0], yavant, q[1]));
    }
    fclose(fich); //Fichier
    flush_to_JSON(trajectoire, tailleTraj / 2);
    return (EXIT_SUCCESS);
}

